#include "main.h"
#include "motor.h"


int Encoder_Left,Encoder_Right;
float PID_Velocity_Kp=-65,PID_Velocity_Ki=-65/200;//PID_Velocity_Kp=-50.5,PID_Velocity_Ki=-51/200;
float PID_Banlance_Kp=-290,PID_Banlance_Kd=-16;//PID_Banlance_Kp=-290,PID_Banlance_Kd=-16;
int Banlance_PID_out,Velocity_PID_out;

extern float angle[3],Velo[3];

void Motor_Init()
{
	HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_3);
	HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_2);
}

void Motor_Left_GetCounter()
{
	Encoder_Left=-(short)TIM3 -> CNT;
	TIM3 -> CNT=0;
}



int Motor_Right_GetCounter()
{
	Encoder_Right=(short)TIM2 -> CNT;
	TIM2 -> CNT=0;
}


///三个量分别是角度，机械中值，角速度
int Banlance_PID(float Angle,float Mechanical_balance,float Gyro)
{
    float Bias;
    static float last_Angle;
	float out_angle=0;
	out_angle=0.7*Angle+0.3*last_Angle;
	last_Angle=out_angle;
    Bias = out_angle - Mechanical_balance;
    Banlance_PID_out =PID_Banlance_Kp*Bias+PID_Banlance_Kd*Gyro;
 return Banlance_PID_out;
}

/*****************  
速度环PI控制器：Kp*Ek+Ki*Ek_S(Ek_S：偏差的积分)
******************/
int Velocity_PID(int Target,int Encoder_Left,int Encoder_Right)
{
  // 定义成静态变量，保存在静态存储器，使得变量不丢掉
  static int Encoder_Err,Encoder_S,EnC_Err_Lowout,EnC_Err_Lowout_last;
  float a=0.7;
  
  // 1.计算速度偏差
  //舍去误差--我的理解：能够让速度为"0"的角度，就是机械中值。
  Encoder_Err = ((Encoder_Left+Encoder_Right)-Target);
  
  // 2.对速度偏差进行低通滤波
  // low_out = (1-a)*Ek+a*low_out_last
  EnC_Err_Lowout = (1-a)*Encoder_Err + a*EnC_Err_Lowout_last; // 使得波形更加平滑，滤除高频干扰，放置速度突变
  EnC_Err_Lowout_last = EnC_Err_Lowout;   // 防止速度过大影响直立环的正常工作
  
  // 3.对速度偏差积分出位移
  Encoder_S+=EnC_Err_Lowout;
  
  // 4.积分限幅
  Encoder_S=Encoder_S>10000?10000:(Encoder_S<(-10000)?(-10000):Encoder_S);
  
  // 5.速度环控制输出
  Velocity_PID_out = PID_Velocity_Kp*EnC_Err_Lowout+PID_Velocity_Ki*Encoder_S;
  
  return Velocity_PID_out;
}

int cnt=0;
void Motor_Out()
{
	cnt++;
	if(cnt<300) return ;
	int pwm=Velocity_PID(0,Encoder_Left,Encoder_Right)+Banlance_PID(angle[1],-0.6,Velo[1]);//
	if(pwm>0&&pwm<210) pwm=220;
	if(pwm<0&&pwm>-210) pwm=-220;
	if(pwm<0)
	{
		AIN1_ON();
		AIN2_DOWN();
		BIN1_ON();
		BIN2_DOWN();
		MotorRight_Pwm(-pwm);
		MotorLeft_Pwm(-pwm);
	}
	else if(pwm>0)
	{
		AIN1_DOWN();
		AIN2_ON();
		BIN1_DOWN();
		BIN2_ON();
		MotorRight_Pwm(pwm);
		MotorLeft_Pwm(pwm);
	}
	
}


///**************************************************************************
//函数功能：速度闭环PID控制(实际为PI控制)
//入口参数：目标速度 当前速度
//返回  值：速度控制值
//根据增量式离散PID公式 
//ControlVelocity+=Kp[e（k）-e(k-1)]+Ki*e(k)+Kd[e(k)-2e(k-1)+e(k-2)]
//e(k)代表本次偏差 
//e(k-1)代表上一次的偏差  以此类推 
//ControlVelocity代表增量输出
//在我们的速度控制闭环系统里面，只使用PI控制
//ControlVelocity+=Kp[e（k）-e(k-1)]+Ki*e(k)
//**************************************************************************/
//int Velocity_FeedbackControl(int TargetVelocity, int CurrentVelocity)
//{
//		int Bias;  //定义相关变量
//		static int ControlVelocity, Last_bias; //静态变量，函数调用结束后其值依然存在
//		
//		Bias=TargetVelocity-CurrentVelocity; //求速度偏差
//		
//		ControlVelocity+=Velcity_Kp*(Bias-Last_bias)+Velcity_Ki*Bias;  //增量式PI控制器
//                                                                   //Velcity_Kp*(Bias-Last_bias) 作用为限制加速度
//	                                                                 //Velcity_Ki*Bias             速度控制值由Bias不断积分得到 偏差越大加速度越大
//		Last_bias=Bias;	
//		return ControlVelocity; //返回速度控制值
//}



///**************************************************************************
//函数功能：位置式PID控制器
//入口参数：目标圈数(位置) 当前位置
//返回  值：速度控制值
//根据位置式离散PID公式 
//pwm=Kp*e(k)+Ki*∑e(k)+Kd[e（k）-e(k-1)]
//e(k)代表本次偏差 
//e(k-1)代表上一次的偏差  
//∑e(k)代表e(k)以及之前的偏差的累积和;其中k为1,2,,k;
//pwm代表输出
//**************************************************************************/
//int Position_FeedbackControl(float Circle, int CurrentPosition)
//{
//		float TargetPosition,Bias, ControlVelocity;     //定义相关变量
//		static float Last_bias, Integral_Bias;          //静态变量，函数调用结束后其值依然存在
//		
//	  TargetPosition=Circle*1040*1.04; //目标位置=目标圈数*1040
//	                              //10ms读取一次编码器(即100HZ)，电机减速比为20，霍尔编码器精度13，AB双相组合得到4倍频，
//	                              //则转1圈编码器读数为20*13*4=1040，电机转速=Encoder*100/1040r/s 使用定时器2
//	                              //1.04是误差系数，电机本身存在误差，可根据实际情况调整该系数以提高控制精度
//	
//		Bias=TargetPosition-CurrentPosition; //求位置偏差
//	  Integral_Bias+=Bias;
//    if(Integral_Bias> 970) Integral_Bias= 970;	//积分限幅 防止到达目标位置后过冲
//	  if(Integral_Bias<-970) Integral_Bias=-970;	//积分限幅 防止到达目标位置后过冲
//	
//		ControlVelocity=Position_Kp*Bias+Position_Ki*Integral_Bias+Position_Kd*(Bias-Last_bias);  //增量式PI控制器
//	                                                                                            //Position_Kp*Bias 偏差越大速度越大
//	                                                                                            //Position_Ki*Integral_Bias 减小稳态误差
//	                                                                                            //Position_Kd*(Bias-Last_bias) 限制速度

//		Last_bias=Bias;	
//		return ControlVelocity;    //返回速度控制值 
//}


// 
///**************************************************************************
//函数功能：速度(PWM)限幅
//入口参数：PWM_P:位置环输出的PWM值 TargetVelocity:目标速度(速度限制值)
//返回  值：无
//**************************************************************************/
//int PWM_Restrict(int PWM_P, int TargetVelocity)
//{
//    if     (PWM_P>+TargetVelocity*76) PWM_P=+TargetVelocity*76;
//	  else if(PWM_P<-TargetVelocity*76) PWM_P=-TargetVelocity*76;
//	  else PWM_P=PWM_P;
//	  return PWM_P;
//}


